/*
author:杨文超
email:yangwenchao@keynection.cn
*/
#include "geminai_modbus.h"
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <stdint.h>                 // for standard int types definition
#include <stddef.h>                 // for NULL and std defines
#include <time.h>
#include <sys/time.h>
#include <string.h>                 // for strerror()
#include <errno.h>                  // for errno
#include "esp_log.h"
#include "geminai_log.h"
#include "freertos/idf_additions.h"
#include "mbport.h"
#include "mbproto.h"
#include "esp_err.h"                // for error handling
#include "driver/uart.h"            // for uart port number defines
#include "sdkconfig.h"              // for KConfig options
#include "geminai_modbus_master.h"
#include "cJSON.h"
#include "string.h"
#include <arpa/inet.h>
#include "geminai_vision_1106.h"
#include "geminai_para.h"
#include "geminai_4g_ap.h"


#define MB_PORT_NUM     (CONFIG_GMB_UART_PORT_NUM)   // Number of UART port used for Modbus connection
#define MB_DEV_SPEED    (230400)  // The communication speed of the UART
#define PACKAGE_MAX_SIZE  (2000)

// The number of parameters that intended to be used in the particular control process
#define MASTER_MAX_CIDS num_device_parameters

// Timeout to update cid over Modbus
#define UPDATE_CIDS_TIMEOUT_MS          (10000)
#define UPDATE_CIDS_TIMEOUT_TICS        (UPDATE_CIDS_TIMEOUT_MS / portTICK_PERIOD_MS)

// Timeout between polls
#define POLL_TIMEOUT_MS                 (100000)
#define POLL_TIMEOUT_TICS               (POLL_TIMEOUT_MS / portTICK_PERIOD_MS)

reboot_args reboot_arg={0};
frp_args frp_arg={0};
software_update_args_t software_update_arg={0};
static const char *TAG = "GEMINAI_MODBUS";

// Enumeration of modbus device addresses accessed by master device
enum {
    MB_DEVICE_ADDR1 = 2 // Only one slave device used for the test (add other slave addresses here)
};

// Enumeration of all supported CIDs for device (used in parameter definition table)

// 在文件开头定义内存池
#define JSON_BUFFER_SIZE 1024
#define CMD_BUFFER_SIZE 256

typedef struct {
    mb_param_request_t request;
    char recv_buffer[256];
    uint16_t version_cmd[128];
    char json_buffer[JSON_BUFFER_SIZE];
} ModbusBuffers;

// static SemaphoreHandle_t modbus_mutex = NULL;
// 创建一个内存池
static ModbusBuffers* create_modbus_buffers(void) {
    ModbusBuffers* buffers = heap_caps_calloc(1, sizeof(ModbusBuffers), MALLOC_CAP_8BIT);
    if (buffers == NULL) {
        GEMINAI_LOGE(TAG, "Failed to allocate ModbusBuffers");
        return NULL;
    }
    
    // 初始化 request
    buffers->request.slave_addr = MB_DEVICE_ADDR1;
    buffers->request.command = 0x17;
    buffers->request.write_regstart = 255;
    buffers->request.write_regsize = 255;
    buffers->request.reg_start = 0;
    buffers->request.reg_size = 256;
    buffers->request.readbuf = buffers->recv_buffer;
    
    return buffers;
}

static void destroy_modbus_buffers(ModbusBuffers* buffers) {
    if (buffers) {
        heap_caps_free(buffers);
    }
}

void print_memory_info(void) {
    multi_heap_info_t info;
    heap_caps_get_info(&info, MALLOC_CAP_DEFAULT);
    
    GEMINAI_LOGI(TAG, "Free memory: %d bytes", heap_caps_get_free_size(MALLOC_CAP_DEFAULT));
    GEMINAI_LOGI(TAG, "Minimum free memory: %d bytes", heap_caps_get_minimum_free_size(MALLOC_CAP_DEFAULT));
    GEMINAI_LOGI(TAG, "Largest free block: %d bytes", info.largest_free_block);
    GEMINAI_LOGI(TAG, "Total allocated: %d bytes", info.total_allocated_bytes);
    GEMINAI_LOGI(TAG, "Total free: %d bytes", info.total_free_bytes);
}

void swapBytes(uint16_t* array, size_t size) {
    for (size_t i = 0; i < size; ++i) {
        uint16_t value = array[i];
        array[i] = (value << 8) | (value >> 8);
    }
}

// Modbus master initialization
static esp_err_t master_init(void)
{
    // Initialize and start Modbus controller
    mb_communication_info_t comm = {
            .port = MB_PORT_NUM,
            .baudrate = MB_DEV_SPEED,
            .parity = MB_PARITY_NONE
    };
    void* master_handler = NULL;

    esp_err_t err = mbc_master_init(&master_handler);
    MB_RETURN_ON_FALSE((master_handler != NULL), ESP_ERR_INVALID_STATE, TAG,
                                "mb controller initialization fail.");
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE, TAG,
                            "mb controller initialization fail, returns(0x%x).", (int)err);
    err = mbc_master_setup((void*)&comm);
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE, TAG,
                            "mb controller setup fail, returns(0x%x).", (int)err);

    // Set UART pin numbers
    err = uart_set_pin(MB_PORT_NUM, CONFIG_GMB_UART_TXD, CONFIG_GMB_UART_RXD,
                              CONFIG_GMB_UART_RTS, UART_PIN_NO_CHANGE);
    const int uart_timeout = 146;  // 调整合适的超时时间
    uart_set_rx_timeout(MB_PORT_NUM, uart_timeout);
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE, TAG,
        "mb serial set pin failure, uart_set_pin() returned (0x%x).", (int)err);
    err = mbc_master_start();
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE, TAG,
                            "mb controller start fail, returned (0x%x).", (int)err);

    // Set driver mode to Half Duplex
    err = uart_set_mode(MB_PORT_NUM, UART_MODE_RS485_HALF_DUPLEX);
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE, TAG,
            "mb serial set mode failure, uart_set_mode() returned (0x%x).", (int)err);

    vTaskDelay(5);
    return err;
}
#if 0
typedef struct {
    uint8_t slave_addr;             
    uint8_t command;                
    uint16_t reg_start;            
    uint16_t reg_size;             
} mb_param_request_t;
#endif

cJSON* set_WIFI_cJSON(uint32_t seqNum,uint32_t cmdType,const char* ssid,const char* password,const char* method)
{
    cJSON *root = cJSON_CreateObject();
    cJSON *seqNumItem = cJSON_CreateNumber(seqNum);
    cJSON *cmdTypeItem = cJSON_CreateNumber(cmdType);
    cJSON *ssidItem = cJSON_CreateString(ssid);
    cJSON *passwordItem = cJSON_CreateString(password);
    cJSON *methodItem = cJSON_CreateString(method);
    //cJSON *channelItem = cJSON_CreateString(channel);
    cJSON *ip_address = cJSON_CreateString("192.168.4.2");
    cJSON_AddItemToObject(root, "seqNum", seqNumItem);
    cJSON_AddItemToObject(root, "cmdType", cmdTypeItem);
    cJSON_AddItemToObject(root, "ssid", ssidItem);
    cJSON_AddItemToObject(root, "password", passwordItem);
    cJSON_AddItemToObject(root, "method", methodItem);
    //cJSON_AddItemToObject(root, "channel", channelItem);
    cJSON_AddItemToObject(root, "ip_address", ip_address);
    return root;
}

cJSON* set_heartbeat_cJSON(uint32_t seq_num,uint16_t cmd_type,uint64_t timestamp)
{
    cJSON *root = cJSON_CreateObject();
    cJSON *seq_numItem = cJSON_CreateNumber(seq_num);
    cJSON *cmd_typeItem = cJSON_CreateNumber(cmd_type);
    cJSON *timestampItem = cJSON_CreateNumber(timestamp);
    cJSON_AddItemToObject(root, "seqNum", seq_numItem);
    cJSON_AddItemToObject(root, "cmdType", cmd_typeItem);
    cJSON_AddItemToObject(root, "timestamp", timestampItem);
    return root; 
}
cJSON* NULL_PACKAGE_cJSON(void)
{
    cJSON *root = cJSON_CreateObject();
    cJSON *seq_numItem = cJSON_CreateNumber(0);
    cJSON *cmd_typeItem = cJSON_CreateNumber(255);
    cJSON_AddItemToObject(root, "seqNum", seq_numItem);
    cJSON_AddItemToObject(root, "cmdType", cmd_typeItem);
    return root; 
}
char Timeout = 0;
char wifi_flag = 0;
char connet_flag = 0;
static void modbus_task(mb_param_request_t *request)
{
    // Static allocation to minimize heap usage
    /*mb_param_request_t *request = (mb_param_request_t *)malloc(sizeof(mb_param_request_t));
    if (request == NULL) {
        printf("Failed to allocate memory for request\n");
        return;
    }
    request->slave_addr = MB_DEVICE_ADDR1;
    request->command = 0x17;
    request->write_regstart = 255;
    request->write_regsize = 255;
    request->reg_start = 0;
    request->reg_size = PACKAGE_MAX_SIZE;

    char recbuf[128] = {0}; // Static buffer
    request->readbuf = recbuf;*/
    cJSON *heart_root = set_heartbeat_cJSON(5, 0, 5);
    cJSON *wifi_root = set_WIFI_cJSON(0x00, 0x04, get_ap_ssid(), "geminai&888", "WPA-PSK");

    if (!heart_root || !wifi_root)
    {
        GEMINAI_LOGE(TAG, "%s(%d) Failed to create cJSON objects", __FILE__, __LINE__);
        vTaskDelete(NULL);
    }

    char *heart_to_add = cJSON_PrintUnformatted(heart_root);
    char *wifi_to_add = cJSON_PrintUnformatted(wifi_root);

    cJSON_Delete(heart_root);
    cJSON_Delete(wifi_root);

    if (!heart_to_add || !wifi_to_add)
    {
        GEMINAI_LOGE(TAG, "%s(%d) Failed to print cJSON objects", __FILE__, __LINE__);
        vTaskDelete(NULL);
    }

    // char heart_len = strlen(heart_to_add);
    char wifi_len = strlen(wifi_to_add);

    /*uint16_t heart_cmd[64] = {0xFCFD, 0xFAFB, 0x0064, heart_len};
    memcpy(heart_cmd + 4, heart_to_add, heart_len);
    swapBytes((uint16_t *)heart_cmd, 64);
    for(int i = 0; i < 64; i++)
    {
        printf("%04x ", heart_cmd[i]);
    }
    printf("\n");*/
    uint16_t wifi_cmd[128] = {0xFCFD, 0xFAFB, 0x0064, wifi_len};
    memcpy(wifi_cmd + 4, wifi_to_add, wifi_len);
    swapBytes((uint16_t *)wifi_cmd, 128);
    free(heart_to_add);
    heart_to_add = NULL;
    free(wifi_to_add);
    wifi_to_add = NULL;

        // Process heart_beat
        /*vTaskDelay(5000 / portTICK_PERIOD_MS);
        memset(recbuf, 0, 128);
        GEMINAI_LOGI(TAG, "heart_beat start");
        esp_err_t ret = mbc_master_send_request(&request, heart_cmd);
        if( ret != ESP_OK)
        {
            Timeout++;
            continue;
        }
        Timeout = 0;
        swapBytes((uint16_t *)recbuf, 64);
        memset(recbuf, 0, 128);*/

    if((Timeout >=10)||(wifi_flag == 0))
{   
    if(Timeout >=10)
    {
        printf("heart_beat timeout..\n");
    }
    vTaskDelay(2000 / portTICK_PERIOD_MS);
    printf("wifi_config start..\n");
    esp_err_t ret = mbc_master_send_request(request, wifi_cmd);
    if( ret != ESP_OK)
    {
        Timeout++;
    }
    else
    {
        Timeout = 0;
        printf("wifi_config success..\n");
    }
    printf("wifi_config end..\n");
    wifi_flag = 1;
    vTaskDelay(2000 / portTICK_PERIOD_MS);
}
}
cJSON* reverse_JSON(uint32_t seqNum, uint32_t cmdType)
{
    cJSON *root = cJSON_CreateObject();
    cJSON_AddNumberToObject(root, "seqNum", seqNum);
    cJSON_AddNumberToObject(root, "cmdType", cmdType);
    return root;
}
cJSON* event_json(uint32_t seqNum, uint32_t cmdType,uint32_t result)
{
    cJSON *root = cJSON_CreateObject();
    cJSON_AddNumberToObject(root, "seqNum", seqNum);
    cJSON_AddNumberToObject(root, "cmdType", cmdType);
    cJSON_AddNumberToObject(root, "result", result);
    //cJSON_AddItemToObject(root, "eventData", eventData);
    return root;
}
static void event_resp(uint32_t seqNum, uint32_t cmdType, uint32_t result)
{
    cJSON *root = event_json(seqNum,cmdType,result);
    char* to_add = cJSON_PrintUnformatted(root);
    cJSON_Delete(root);
    char event_len = strlen(to_add);
    uint16_t event_cmd[255] = {0xFCFD, 0xFAFB, 0x0065, event_len};
    memcpy(event_cmd + 4, to_add, event_len);
    swapBytes((uint16_t *)event_cmd, 255);
    free(to_add);
    to_add = NULL;
    mb_param_request_t *request = (mb_param_request_t *)malloc(sizeof(mb_param_request_t));
    if (request == NULL)
    {
        printf("Failed to allocate memory for request\n");
        return;
    }
    request->slave_addr = MB_DEVICE_ADDR1;
    request->command = 0x17;
    request->write_regstart = 255;
    request->write_regsize = 255;
    request->reg_start = 0;
    request->reg_size = 256/2;
    void *recbuf = malloc(256);
    memset(recbuf, 0,256);
    request->readbuf = (char *)recbuf;
    mbc_master_send_request(request, event_cmd);
    free(recbuf);
    free(request);
}
#define PACKTOTAL_SIZE 65536
char *strbuf;
EXT_RAM_BSS_ATTR char recbuf[PACKTOTAL_SIZE+2];
char* geteventdata(const char *str) {
    int depth = 0;
    const char *start = NULL;
    const char *end = NULL;

    for (int i = 0; str[i] != '\0'; i++) 
    {
        if (str[i] == '{') 
        {
            if (depth == 0) 
            {
                start = &str[i]; // 记录开始位置（包括'{'）
            }
            depth++;
        } else if (str[i] == '}') 
        {
            depth--;
            if (depth == 0 && start != NULL) 
            {
                end = &str[i]; // 记录结束位置（包括 '}'）
                break; // 找到最外层花括号，退出循环
            }
        }
    }

    if (start && end) 
    {
        size_t length = end - start + 1;
        
        char asset_id[17+strlen(base_station.asset_id)] ;
        memset(asset_id, 0, 17+strlen(base_station.asset_id));
        sprintf(asset_id, "{\"asset_id\":\"%s\",", base_station.asset_id);
        char *result = (char *)malloc(length + strlen(asset_id));
        if (result) 
        {
            memcpy(result, asset_id, strlen(asset_id));
            memcpy(result+strlen(asset_id), start+1, length-1);
            result[length + strlen(asset_id) - 1] = '\0'; // 添加字符串结束符
            return result;
        }
    }

    return NULL; // 如果没有找到有效内容，返回 NULL
}
char* geteventType(const char *str) 
{
    char *eventType = NULL;
    char *start = NULL;
    char *end = NULL;

    start = strstr(str, "\"eventType\":");
    if (start != NULL) 
    {
        start += strlen("\"eventType\":\"");
        end = strchr(start, ',');
        if (end != NULL) 
        {
            size_t length = end - start - 1;
            eventType = (char *)malloc(length + 1);
            if (eventType != NULL) 
            {
                strncpy(eventType, start, length);
                eventType[length] = '\0';
                return eventType;
            }
        }
    }
    return NULL;
}
int getseqNum(const char *str)
{
    int seqNum = 0;
    char *start = NULL;
    char *end = NULL;

    start = strstr(str, "\"seqNum\":");
    if (start != NULL)
    {
        end = strchr(start, ',');
        if (end != NULL)
        {   
            start += strlen("\"seqNum\":");
            seqNum = atoi(start);
        }
        else
        {
            sscanf(start, "\"seqNum\":%d", &seqNum);
        }
    }
    return seqNum;
}

uint32_t heartbeat_count = 0;
uint32_t global_event_count = 0;
uint32_t global_rpc_count = 0;
bool debug_switch = false;
uint16_t reverse_cmd[255] = {0xFCFD, 0xFAFB, 0x0064};
uint16_t nullcmd[255] = {0xFCFD, 0xFAFB, 0x0064};
static void demo_task(void *pvParameters)
{   
    uint16_t json1len = 0;
    uint16_t json2len = 0;
    cJSON* root = reverse_JSON(0x00, 0x02);
    char* to_add = cJSON_PrintUnformatted(root);
    printf("\n****** demo_task START ******\n");
    cJSON_Delete(root);
    if (to_add == NULL) {
        printf("Failed to create JSON string\n");
        return;
    }
    char reverse_len = strlen(to_add);
    if (reverse_len > 255 - 4) { // Ensure we don't overflow
        printf("Data too large for reverse_cmd buffer\n");
        free(to_add);
        to_add = NULL;
        return;
    }
    
    reverse_cmd[3] = reverse_len;
    memcpy(reverse_cmd + 4, to_add, reverse_len);
    swapBytes((uint16_t *)reverse_cmd, 255);
    free(to_add);
    to_add = NULL;
    mb_param_request_t *request = (mb_param_request_t *)malloc(sizeof(mb_param_request_t));
    if (request == NULL) {
        printf("Failed to allocate memory for request\n");
        return;
    }
    request->slave_addr = MB_DEVICE_ADDR1;
    request->command = 0x17;
    request->write_regstart = 255;
    request->write_regsize = 255;
    request->reg_start = 0;
    request->reg_size = PACKAGE_MAX_SIZE;
    printf("recv = %p\n", recbuf);
    uint16_t data_len = 0;
    request->readbuf = recbuf;
    cJSON* cmd = NULL_PACKAGE_cJSON();
    char* cmd_str = cJSON_PrintUnformatted(cmd);
    char nulllen = strlen(cmd_str);

    nullcmd[3] = nulllen;
    memcpy(nullcmd + 4, cmd_str, nulllen);
    swapBytes((uint16_t *)nullcmd, 255);
    printf("\n");
    free(cmd);
    cmd = NULL;
    free(cmd_str);
    cmd_str = NULL;
    strbuf = malloc(PACKTOTAL_SIZE);
    uint16_t recv_len = 0;
    while (1) 
    {
        json1len = 0;
        json2len = 0;

        memset(strbuf, 0, PACKTOTAL_SIZE);
        // 发送请求
        esp_err_t ret;
        ret = mbc_master_send_request(request, reverse_cmd);
        if(ret!= ESP_OK)
        {   Timeout++;
            // printf("mbc_master_send_request failed\n");
            continue;
        }
        // 读取响应长度
        memcpy(&data_len, recbuf, 2);
        // printf("Initial strbuf length: %d\n", data_len);
        swapBytes((uint16_t *)&recbuf,PACKAGE_MAX_SIZE+2/2);
        /*memcpy(strbuf+recv_len, recbuf + 2, 1000);
        memcpy(strbuf+recv_len+1000, recbuf+1000 + 2, PACKAGE_MAX_SIZE-1000);

        printf("*****recbuf****\n");
        for(int i = 0; i < PACKAGE_MAX_SIZE+2; i++) 
        {
            printf("%02x ", recbuf[i]);        
        }
        printf("\n****************\n");

        printf("*****strbuf****\n");
        for(int i = 0; i < data_len ; i++) 
        {
            printf("%02x ", strbuf[i]);        
        }
        printf("\n****************\n");*/

        // 确保接收循环能正确处理数据
    
        while (data_len > 0) 
        {   
            vTaskDelay(10 / portTICK_PERIOD_MS);
            // 检查 data_len 是否有效
            if (data_len >= PACKAGE_MAX_SIZE) 
            {                // 复制数据
                //printf("recbuf = %p,strbuf %p  + recv_len = %d\n",recbuf,strbuf, recv_len);
                memcpy(strbuf+recv_len, recbuf + 2, PACKAGE_MAX_SIZE);

                recv_len += data_len;
                //printf("recv_len =: %d \n", recv_len);
                // 重置 recbuf
                memset(recbuf, 0, PACKAGE_MAX_SIZE + 2);
                printf("***** strbuf ****\n");
                // 发送请求以接收更多数据
                ret = mbc_master_send_request(request, nullcmd);
                if(ret != ESP_OK)
                {   Timeout++;
                    // printf("mbc_master_send_request failed\n");
                    continue;
                }
                // 再次读取数据长度
                memcpy(&data_len, recbuf, 2);
                swapBytes((uint16_t *)&recbuf,PACKAGE_MAX_SIZE+2/2);
                printf("Receiving remaining data: %d bytes\n", data_len);
            }
            else 
            {
                // 处理最后一部分数据
                // printf("recbuf = %p,strbuf %p  + recv_len = %d\n",recbuf,strbuf, recv_len);
                memcpy(strbuf+recv_len, recbuf + 2, data_len);
                recv_len += data_len;
                break;
            }
        }
        // 打印接收到的数据
        //printf("Receiving all data: %d bytes\n", recv_len);
        //printf("strbuf = %p\n",strbuf);
        //for(uint16_t i = 0; i < recv_len; i++) 
        //{
        //    printf("%02x ", strbuf[i]);
        //}
        // printf("\n");
        recv_len = 0;
        memcpy(&json1len,strbuf+6,2);
        // printf("cmdlen:%d\n",json1len);
        if(json1len == 0)
        {
            vTaskDelay(200 / portTICK_PERIOD_MS);
            continue;
        }
        memcpy(&json2len,strbuf+8+json1len+6,2);
        if(json2len == 0)
        {   
            heartbeat_count++;
            if (debug_switch) {
                printf("heart beat %ld\n",heartbeat_count);
            }
            vTaskDelay(200 / portTICK_PERIOD_MS);
            modbus_task(request);
            continue;
        }
        printf("eventlen:%d\n",json2len);
        char *json2str = malloc(json2len+1);
        memset(json2str,0,json2len+1);
        memcpy(json2str,strbuf+8+json1len+8,json2len);
        printf("%s",json2str);
        printf("\n****************\n");
        char* eventdata = strstr(json2str,"\"eventData\":");
        eventdata = eventdata + strlen("\"eventData\":");
        char* eventdata_str = geteventdata(eventdata);
        if(eventdata_str != NULL)
        {
            printf("eventdata:%s\n",eventdata_str);
        }
        else
        {
            printf("eventdata is null\n");
            free(json2str);
            json2str = NULL;
            vTaskDelay(200 / portTICK_PERIOD_MS);
            continue;
        }
        char* eventType = geteventType(json2str);
        if(eventType!=NULL)
        {
            printf("eventType:%s\n",eventType);
        }
        else
        {
            printf("eventType is null\n");
            free(json2str);
            json2str = NULL;
            vTaskDelay(200 / portTICK_PERIOD_MS);
            continue;
        }
        int seqNum = getseqNum(json2str);
        if(analyze_vision_event(eventType, eventdata_str))
        {
           event_resp(seqNum, 0x61, 0x01);
           free(eventdata_str);        
        }
        else
        {
           event_resp(seqNum, 0x61, 0x00);
        }
        global_event_count++;
        Timeout = 0;
        vTaskDelay(200 / portTICK_PERIOD_MS);
    }
    printf("\n****** demo_task end ******\n");
    free(strbuf);
    free(recbuf);
    free(request);
    free(cmd_str);
    vTaskDelete(NULL);
}
cJSON* Command_cJSON(uint32_t timestamp,uint32_t cmdType,cJSON *info,uint32_t seqNum)
{
    cJSON *root = cJSON_CreateObject();
    cJSON_AddNumberToObject(root, "timestamp",timestamp);
    cJSON_AddNumberToObject(root, "cmdType", cmdType);
    cJSON_AddNumberToObject(root, "seqNum", seqNum);
    if(info!=NULL)
    {
        cJSON_AddItemToObject(root, "info", info);
    }
    return root;
}
cJSON* self_check_json(uint32_t seqNum,uint32_t cmdType,uint32_t timestamp)
{
    cJSON *root = cJSON_CreateObject();
    cJSON_AddNumberToObject(root, "seqNum", seqNum);
    cJSON_AddNumberToObject(root, "cmdType", 0x0E);
    cJSON_AddNumberToObject(root, "timestamp", timestamp);
    return root;
}
cJSON* reboot_json(uint32_t cmdType,uint32_t seqNum,uint32_t delay,uint32_t timestamp)
{
    cJSON *root = cJSON_CreateObject();
    cJSON_AddNumberToObject(root, "cmdType", cmdType);
    cJSON_AddNumberToObject(root, "seqNum", seqNum);
    cJSON_AddNumberToObject(root, "delay", delay);
    cJSON_AddNumberToObject(root, "timestamp", timestamp);
    return root;
}
cJSON* frp_cJSON(uint32_t seqNum,const char* frp_ip,uint32_t listen_port,uint32_t comm_port)
{
    cJSON *root = cJSON_CreateObject();
    cJSON_AddNumberToObject(root, "seqNum", seqNum);
    cJSON_AddStringToObject(root, "frp_ip", frp_ip);
    cJSON_AddNumberToObject(root, "listen_port", listen_port);
    cJSON_AddNumberToObject(root, "cmdType", 0x16);
    cJSON_AddNumberToObject(root, "comm_port", comm_port);
    return root;
}
cJSON* software_update_cJSON(const char* type,const char* firmware_url,const char* firmware_md5,uint16_t upgrade_now,const char* version_current,uint32_t seqNum)
{
    cJSON *root = cJSON_CreateObject();
    cJSON_AddNumberToObject(root, "seqNum", seqNum);
    cJSON_AddNumberToObject(root, "cmdType", 0x08);
    cJSON_AddStringToObject(root, "type", type);
    cJSON_AddStringToObject(root, "firmware_url", firmware_url);
    cJSON_AddStringToObject(root, "firmware_md5", firmware_md5);
    cJSON_AddNumberToObject(root, "upgrade_now", upgrade_now);
    cJSON_AddStringToObject(root, "version_current", version_current);
    return root;
}
cJSON* take_photo_cJSON(uint32_t cmdType,uint32_t seqNum,uint32_t channel,const char* client_sn,const char* type)
{
    cJSON *root = cJSON_CreateObject();
    cJSON_AddNumberToObject(root, "cmdType", cmdType);
    cJSON_AddNumberToObject(root, "seqNum", seqNum);
    cJSON_AddNumberToObject(root, "channel", channel);
    cJSON_AddStringToObject(root, "client_sn", client_sn);
    cJSON_AddStringToObject(root, "type", type);
    return root;
}
void defcommnd(cJSON* cmd) {
    if (cmd == NULL) {
        GEMINAI_LOGE(TAG, "Invalid JSON object");
        return;
    }

    ModbusBuffers* buffers = create_modbus_buffers();
    if (buffers == NULL) {
        GEMINAI_LOGE(TAG, "Failed to create modbus buffers");
        return;
    }

    // 使用 PrintPreallocated 避免动态分配
    if (!cJSON_PrintPreallocated(cmd, buffers->json_buffer, JSON_BUFFER_SIZE, false)) {
        GEMINAI_LOGE(TAG, "Failed to print JSON to buffer");
        goto cleanup;
    }

    // 获取字符串长度并检查
    size_t version_len = strlen(buffers->json_buffer);
    if (version_len > ((128 - 4) * 2)) {
        GEMINAI_LOGE(TAG, "JSON string too long: %d", version_len);
        goto cleanup;
    }

    // 初始化命令数组
    buffers->version_cmd[0] = 0xFCFD;
    buffers->version_cmd[1] = 0xFAFB;
    buffers->version_cmd[2] = 0x0064;
    buffers->version_cmd[3] = version_len;

    // 复制数据
    memcpy((char*)(&buffers->version_cmd[4]), buffers->json_buffer, version_len);

    // 字节交换
    swapBytes(buffers->version_cmd, 128);

    // 发送请求
    esp_err_t err = mbc_master_send_request(&buffers->request, buffers->version_cmd);
    swapBytes((uint16_t*)buffers->recv_buffer,256);
    if (err != ESP_OK) {
        GEMINAI_LOGE(TAG, "Failed to send request: %s", esp_err_to_name(err));
    } else {
        GEMINAI_LOGI(TAG, "Transfer complete");
        uint16_t cmdlen =0;
        memcpy(&cmdlen,buffers->recv_buffer+8,2);
        printf("cmdlen = %d\n",cmdlen);
        char* str = malloc(cmdlen+1);
        memset(str,0,cmdlen+1);
        memcpy(str,buffers->recv_buffer+10,cmdlen);
        printf("recv str = \n %s\n",(const char*)str);
        free(str);
        str = NULL;
    }

cleanup:
    destroy_modbus_buffers(buffers);
}

cJSON* iot_auth_req_cJSON(uint32_t seqNum,uint32_t cmdType,const char* iot,const char* oss,const char* module)
{
    cJSON *root = cJSON_CreateObject();
    cJSON_AddNumberToObject(root, "cmdType", cmdType);
    cJSON_AddNumberToObject(root, "seqNum", seqNum);
    cJSON_AddStringToObject(root, "iot", iot);
    cJSON_AddStringToObject(root, "oss", oss);
    cJSON_AddStringToObject(root, "module", module);
    return root;
}
void self_check(void)
{
    printf("self_check\n");
    defcommnd(self_check_json(0,0x0E,0x10000));
}
void software_version_get(uint32_t seqNum)
{
    printf("software_version_get\n");
    defcommnd(Command_cJSON(seqNum,0x0C,NULL,seqNum));  
}
void iot_auth_req(uint32_t seqNum)
{
    printf("iot_auth_req\n");
    defcommnd(iot_auth_req_cJSON(seqNum,0x0E,"iot","oss","module"));
}
void reboot_cmd(void* arg)
{
    reboot_args* args = (reboot_args*)arg;
    printf("reboot_cmd\n");
    defcommnd(reboot_json(0x10,args->seqNum,args->timestamp,args->timestamp));
}
void take_photo_cmd(uint32_t seqNum)
{
    printf("take_photo_cmd\n");
    defcommnd(take_photo_cJSON(0x14,seqNum,255,"client_sn_000001","mjpeg"));
}
void software_update(void* arg)
{
    software_update_args_t* args = (software_update_args_t*)arg;
    GEMINAI_LOGI(TAG, "software_update");
    
    // 参数验证
    if (args == NULL || args->type == NULL || args->firmware_url == NULL || 
        args->firmware_md5 == NULL || args->version_current == NULL) {
        GEMINAI_LOGE(TAG, "Invalid software_update_args");
        return;
    }

    cJSON *root = cJSON_CreateObject();
    if (root == NULL) {
        GEMINAI_LOGE(TAG, "Failed to create JSON object");
        return;
    }

    bool success = true;
    do {
        if (!cJSON_AddNumberToObject(root, "seqNum", 0)) break;
        if (!cJSON_AddNumberToObject(root, "cmdType", 0x08)) break;
        if (!cJSON_AddStringToObject(root, "type", args->type)) break;
        if (!cJSON_AddStringToObject(root, "firmware_url", args->firmware_url)) break;
        if (!cJSON_AddStringToObject(root, "firmware_md5", args->firmware_md5)) break;
        if (!cJSON_AddNumberToObject(root, "upgrade_now", args->upgrade_now)) break;
        if (!cJSON_AddStringToObject(root, "version_current", args->version_current)) break;
        success = true;
    } while(0);

    if (!success) {
        GEMINAI_LOGE(TAG, "Failed to add items to JSON object");
        cJSON_Delete(root);
        return;
    }

    defcommnd(root);
    cJSON_Delete(root);
}
void frp(void* arg)
{
    frp_args* args = (frp_args*)arg;
    GEMINAI_LOGI(TAG, "frp command start");
    
    // 参数验证
    if (args == NULL || args->frp_ip == NULL) {
        GEMINAI_LOGE(TAG, "Invalid frp_args");
        return;
    }

    // 打印参数信息
    GEMINAI_LOGI(TAG, "FRP params - IP: %s, Listen: %ld, Comm: %ld",
             args->frp_ip, args->listen_port, args->comm_port);

    cJSON *root = cJSON_CreateObject();
    if (root == NULL) {
        GEMINAI_LOGE(TAG, "Failed to create JSON object");
        return;
    }

    bool success = true;
    do {
        if (!cJSON_AddNumberToObject(root, "seqNum", 0)) break;
        if (!cJSON_AddStringToObject(root, "frp_ip", args->frp_ip)) break;
        if (!cJSON_AddNumberToObject(root, "listen_port", args->listen_port)) break;
        if (!cJSON_AddNumberToObject(root, "cmdType", 0x16)) break;
        if (!cJSON_AddNumberToObject(root, "comm_port", args->comm_port)) break;
        success = true;
    } while(0);

    if (!success) {
        GEMINAI_LOGE(TAG, "Failed to add items to JSON object");
        cJSON_Delete(root);
        return;
    }

    // 调用 defcommnd 发送命令
    defcommnd(root);
    
    // 清理资源
    cJSON_Delete(root);
    
    GEMINAI_LOGI(TAG, "frp command complete");
}
// 初始化队列
void initQueue(TaskQueue *q) {
    q->front = 0;
    q->rear = 0;
    q->size = 0;
}

// 检查队列是否已满
int isFull(TaskQueue *q) {
    return q->size == MAX_QUEUE_SIZE;
}

// 检查队列是否为空
int isEmpty(TaskQueue *q) {
    return q->size == 0;
}

// 将任务加入队列
int enqueue(TaskQueue *q, Task task) {
    if (isFull(q)) {
        printf("Queue is full, cannot add more tasks.\n");
        return -1;
    }
    q->queue[q->rear] = task;  // 存储任务函数
    q->rear = (q->rear + 1) % MAX_QUEUE_SIZE;
    q->size++;
    return 0;
}

// 从队列中取出任务
Task dequeue(TaskQueue *q) {
    Task task = {NULL, NULL};
    if (isEmpty(q)) {
        return task;
    }
    task = q->queue[q->front];
    q->front = (q->front + 1) % MAX_QUEUE_SIZE;
    q->size--;
    return task;
}
TaskQueue mb_task_queue;
void runloop_task(void *param) {
    GEMINAI_LOGI(TAG, "runloop_task started");

    initQueue(&mb_task_queue);
    while (1) {
        Task task = dequeue(&mb_task_queue);
        if (task.function == NULL)
        {
            vTaskDelay(pdMS_TO_TICKS(200));
            continue;
        }
        global_rpc_count++;
        task.function(task.arg);
    }
}

static TaskHandle_t runloop_task_handle = NULL;
static TaskHandle_t demo_task_handle = NULL;

void modbus_task_start(void)
{
    register_cmd_modbus();
    ESP_ERROR_CHECK(master_init());
    // Initialization of device peripheral and objects
    // Initialization of device peripheral and objects
    vTaskDelay(10);
    printf("modbus_task_start\n");

    xTaskCreate(runloop_task, "runloop_task", 4096, NULL, 5, &runloop_task_handle);
    // xTaskCreate(demo_task, "demo_task", 8192, NULL, 12, &demo_task_handle);
}
void modbus_task_stop(void)
{
    if(runloop_task_handle != NULL)
    {
        vTaskDelete(runloop_task_handle);
        runloop_task_handle = NULL;
    }
    if(demo_task_handle != NULL)
    {
        vTaskDelete(demo_task_handle);
        demo_task_handle = NULL;
    }
}


static int modbus_cmd(int argc, char **argv)
{
    if (argc < 2) {
        printf("heartbeat_count     = %ld\n", heartbeat_count);
        printf("global_event_count  = %ld\n", global_event_count);
        printf("global_rpc_count    = %ld\n", global_rpc_count);
        return 0;
    }

    if (strcmp(argv[1], "debug") == 0) {
        if (argc >= 3) {
            if (strcmp(argv[2], "on") == 0) {
                debug_switch = true;
                printf("debug_switch on\n");
            } else if (strcmp(argv[2], "off") == 0) {
                debug_switch = false;
                printf("debug_switch off\n");
            }
        }
    } else if (strcmp(argv[1], "stop") == 0) {
        printf("Modbus task stopped.\n");
    } else {
        printf("Usage: modbus <start|stop>\n");
        return 1;
    }

    return 0;
}

static void register_modbus(void)
{
    const esp_console_cmd_t cmd = {
        .command = "modbus",
        .help = "modbus command",
        .hint = NULL,
        .func = &modbus_cmd,
        .argtable = NULL
    };
    ESP_ERROR_CHECK(esp_console_cmd_register(&cmd));
}

void register_cmd_modbus(void)
{
    register_modbus();
}
